// ５个点拟合一个平面
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <vector>

// 用于产生平面点的噪声
#include <random>

// 生成带有噪声的平面上的点(GPT)
Eigen::MatrixXd generatePointsOnPlaneWithNoise(int numPoints, double noiseLevel) {
    // 定义平面参数
    double a = 1.0;
    double b = 2.0;
    double c = -3.0;
    double d = 1.0;

    // 生成符合平面方程的点
    Eigen::MatrixXd points(3, numPoints);

    for (int i = 0; i < numPoints; ++i) {
        double x = static_cast<double>(rand()) / RAND_MAX * 10.0;  // 随机生成x坐标
        double y = static_cast<double>(rand()) / RAND_MAX * 10.0;  // 随机生成y坐标
        double z = (-a * x - b * y - d) / c;  // 计算符合平面方程的z坐标
        points.col(i) << x, y, z;
    }

    // 添加噪声
    Eigen::MatrixXd noise = noiseLevel * Eigen::MatrixXd::Random(3, numPoints);
    points += noise;

    return points;
}


// 使用eigen来解，Ax=B的超定方程来拟合平面，定义平面参数为 ax+by+cz+d=0，然后将d作为１
// 结果：　2.09155  4.25319 -6.19803        1
Eigen::Vector4d plan_fit_eigen(std::vector<Eigen::Vector3d> points){
    Eigen::Vector4d plan_param;
    Eigen::Matrix<double, 5, 3> A;
    Eigen::Matrix<double, 5, 1> B;
    A.setZero();
    B.setOnes();
    B *= -1.0;
    for (int i = 0; i < 5; i++)
    {
        A(i,0) = points[i](0);
        A(i,1) = points[i](1);
        A(i,2) = points[i](2);
    }
    
    // 求解Ax=B方程
    Eigen::Matrix<double, 3, 1> x = A.colPivHouseholderQr().solve(B);
    plan_param[0] = x(0);
    plan_param[1] = x(1);
    plan_param[2] = x(2);
    plan_param[3] = 1.0;

    return plan_param;
}


// 使用SVD分解来计算，其实是不是就是主要成分分析？？？
Eigen::Vector3d plan_fit_svd(std::vector<Eigen::Vector3d> points){
    Eigen::Vector4d plan_param = Eigen::Vector4d::Zero();
    Eigen::Matrix<double, 5, 3> A;
    A.setZero();
    for (int i = 0; i < 5; i++)
    {
        A(i,0) = points[i](0);
        A(i,1) = points[i](1);
        A(i,2) = points[i](2);
    }
    // 对A进行SVD分解
    
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::MatrixXd right_matrix = svd.matrixV();   // 每一列是一个他特征向量
    Eigen::VectorXd values = svd.singularValues();

    // std::cout << "values : " << values.transpose() <<std::endl;
    // std::cout << "right_matrix : " << right_matrix <<std::endl;

    // 这里的right_matrix最后一列就是发现方法，因为特征值会自动排序，但是这里只能知道法向量
    Eigen::Vector3d plan_ = right_matrix.col(2);


    return plan_.normalized();
}

// 主成分分析PCA
Eigen::Vector3d plan_fit_pca(std::vector<Eigen::Vector3d> points){
    Eigen::Matrix<double, 5, 3> A;
    Eigen::Vector3d A_maen = Eigen::Vector3d::Zero();
    A.setZero();
    for (int i = 0; i < 5; i++)
    {
        A_maen(0) = A(i,0) = points[i](0);
        A_maen(1) = A(i,1) = points[i](1);
        A_maen(2) = A(i,2) = points[i](2);
    }

    A_maen /= 5;
    for (int i = 0; i < 5; i++)
        A.row(i) -= A_maen.transpose();

    Eigen::Matrix3d ATA = A.transpose() * A;

    Eigen::EigenSolver<Eigen::MatrixXd> solver(ATA);
    Eigen::Vector3d values = solver.eigenvalues().real();
    Eigen::Matrix3d matrix = solver.eigenvectors().real();

    // std::cout << "values : " << values.transpose() << std::endl;
    // std::cout << "matrix : " << matrix << std::endl;     // 还是最后一列是法向量

    Eigen::Vector3d plan_ = matrix.col(2);
    return plan_.normalized();
}


// 最小二乘法计算，Ax=B，x = (ATA)^-1ATB
Eigen::Vector3d plan_fit_lsq(std::vector<Eigen::Vector3d> points){
    Eigen::Matrix<double, 5, 3> A;
    Eigen::Matrix<double, 5, 1> B;
    A.setZero();
    B.setOnes();
    B *= -1.0;
    for (int i = 0; i < 5; i++)
    {
        A(i,0) = points[i](0);
        A(i,1) = points[i](1);
        A(i,2) = points[i](2);
    }

    Eigen::Vector3d values = (A.transpose() * A).inverse() * (A.transpose() * B);
    // Eigen::Vector3d values_1 = (A).inverse() * (B);

    std::cout << "values : " << values.normalized().transpose() << std::endl;

    return values.normalized();
}



int main(int argc, char* argv[]){
    int number = 5;
    Eigen::MatrixXd points = generatePointsOnPlaneWithNoise(number,0.1);
    // std::cout << "points : " << points << std::endl;
    // 填充数据
    std::vector<Eigen::Vector3d> points_in;
    for (size_t i = 0; i < number; i++)
    {
        Eigen::Vector3d temp_point;
        temp_point(0) = points(0,i);
        temp_point(1) = points(1,i);
        temp_point(2) = points(2,i);
        points_in.push_back(temp_point);
    }

    Eigen::Vector4d plan_eigen = plan_fit_eigen(points_in);
    Eigen::Vector3d plan_svd = plan_fit_svd(points_in);
    Eigen::Vector3d plan_pca = plan_fit_pca(points_in);
    Eigen::Vector3d plan_lsq = plan_fit_lsq(points_in);
    
    std::cout << "plan_svd : " << plan_svd.transpose() << std::endl;

    std::cout << "plan_eigen : " << plan_eigen.head<3>().normalized().transpose() << std::endl;

    std::cout << "plan_pca : " << plan_pca.transpose() << std::endl;

    std::cout << "plan_lsq : " << plan_lsq.transpose() << std::endl;


    return 0;
}






